// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PicknPlaceAgentBase.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Manipulation.Runtime
{
    using System;
    using System.Collections.Generic;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.Manipulation.PicknPlace;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Tracing;

    /// <summary>
    /// Base class for handling pick (and move) and place (and move) requests used for the pick and place task.
    /// The agent decomposes each request into a number of discrete " move and grip" steps.
    /// For each step the desired arm pose and gripper state at the end of the step is computed by the agent's helper classes.
    /// The agent sends control messages to arm and gripper agents that realize the arm and gripper configurations for each step.
    /// The agent monitors state from these agents to determine completion of the requests
    /// </summary>
    /// <typeparam name="TGripperControl">Message type to control the gripper</typeparam>
    /// <typeparam name="TGripperStatus">Message type giving gripper status</typeparam>
    /// <typeparam name="TArmCartesianControl">Message type controlling cartesian motion of the arm</typeparam>
    [DataContract]
    public abstract class PicknPlaceAgentBase<TGripperControl, TGripperStatus, TArmCartesianControl> : ConsumerProducerAgent<ManipulatorStatusMessage, PicknPlaceStatusMessage>, 
                                        ISubscriptionReceiver<TGripperStatus>,
                                        IControlReceiver<PickAndMoveMessage>,
                                        IControlReceiver<PlaceAndMoveMessage>
        where TGripperControl : AgentMessage
        where TGripperStatus : AgentMessage
        where TArmCartesianControl : AgentMessage
    {
        /// <summary>
        /// FSM driving pick from task
        /// </summary>
        private MoveAndGraspFSM pickFromFSM;

        /// <summary>
        /// FSM driving place task
        /// </summary>
        private MoveAndGraspFSM placeToFSM;

        /// <summary>
        /// Arm controller agent name
        /// </summary>
        [DataMember(Name = "ArmControlAgentName")]
        private string armControlAgentName;

        /// <summary>
        /// The default neutral pose used for orienting the tool control point
        /// prior to a move. This is just a temporary crutch to help prevent the arm
        /// from getting into weird joint configurations. It can go away when arm movements become more reliable 
        /// </summary>
        [DataMember(Name = "DefaultNeutralPosition")]
        private Pose defaultNeutralPose;

        /// <summary>
        /// Hand controller agent name
        /// </summary>
        [DataMember(Name = "HandControlAgentName")]
        private string handControlAgentName;

        /// <summary>
        /// hand status message provider
        /// </summary>
        [DataMember(Name = "HandStatusProvider")]
        private string handStatusProvider;

        /// <summary>
        /// Lift height from grasp position
        /// </summary>
        [DataMember(Name = "LiftHeight")]
        private double liftHeight;

        /// <summary>
        /// Initializes a new instance of the <see cref="PicknPlaceAgentBase {TGripperControl,TGripperStatus,TArmCartesianControl}" /> class
        /// </summary>
        /// <param name="name">The name</param>
        /// <param name="producer">The name of the agent we subscribe to.</param>
        /// <param name="armControlAgentName">Name of the arm control agent</param>
        /// <param name="handControlAgentName">Name of the hand control agent</param>
        /// <param name="handStatusProvider">Name of agent providing integrated arm / hand status</param>
        /// <param name="armOrientationMoveTolerance">Tolerance on arm orientation moves</param>
        /// <param name="armPositionMoveTolerance">Tolerance on arm move convergence</param>
        /// <param name="liftHeight">Lift height from grasp position</param>
        /// <param name="defaultSubTaskStepTimeOutmSec">Time out in mSec at each step in pick and place tasks</param>
        /// <param name="gripExchangePose">Location used for grip exchange</param>
        /// <param name="defaultNeutralPose">Default neutral position before a pick or place</param>
        public PicknPlaceAgentBase(
            string name,
            IProducer<ManipulatorStatusMessage> producer,
            string armControlAgentName,
            string handControlAgentName,
            string handStatusProvider,
            double armOrientationMoveTolerance,
            double armPositionMoveTolerance,
            double liftHeight,
            int defaultSubTaskStepTimeOutmSec,
            Pose gripExchangePose,
            Pose defaultNeutralPose)
            : base(name, producer.Name)
        {
            this.armControlAgentName = armControlAgentName;
            this.handControlAgentName = handControlAgentName;
            this.handStatusProvider = handStatusProvider;
            this.ArmOrientationMoveTolerance = armOrientationMoveTolerance;
            this.ArmPositionMoveTolerance = armPositionMoveTolerance;
            this.liftHeight = liftHeight;
            this.DefaultSubTaskStepTimeOutmSec = defaultSubTaskStepTimeOutmSec;
            this.GripExchangePose = gripExchangePose;
            this.defaultNeutralPose = defaultNeutralPose;
        }

        /// <summary>
        /// Gets the hardware dependent control port for managing arm moves
        /// </summary>
        protected IForwarder<TArmCartesianControl> ArmControlAgentPort { get; private set; }

        /// <summary>
        /// Gets tolerance on arm position moves
        /// </summary>
        [DataMember(Name = "ArmPositionMoveTolerance")]
        protected double ArmPositionMoveTolerance { get; private set; }

        /// <summary>
        /// Gets tolerance on arm orientation moves
        /// </summary>
        [DataMember(Name = "ArmOrientationMoveTolerance")]
        protected double ArmOrientationMoveTolerance { get; private set; }

        /// <summary>
        /// Gets the default time out at the end of each step in pick and place sub tasks in mSec
        /// </summary>
        [DataMember(Name = "DefaultSubTaskStepTimeOutmSec")]
        protected int DefaultSubTaskStepTimeOutmSec { get; private set; }

        /// <summary>
        /// Gets the pose for performing grip exchange
        /// </summary>
        [DataMember(Name = "GripExchangePose")]
        protected Pose GripExchangePose { get; private set; }

        /// <summary>
        /// Gets or sets the pick FSM context
        /// </summary>
        protected PicknPlaceFSMContext Context { get; set; }

        /// <summary>
        /// Gets or sets the hardware dependent control port for managing gripper control
        /// </summary>
        protected IForwarder<TGripperControl> HandControlAgentPort { get; set; }

        /// <summary>
        /// Gets or sets the helper to generate pick states
        /// </summary>
        protected PicknPlaceTaskDescriptionGenerator PicknPlaceGenerator { get; set; }

        /// <summary>
        /// Initializes the agent
        /// </summary>
        /// <param name="locator">Agent locator to use</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);

            if (false == string.IsNullOrEmpty(this.armControlAgentName))
            {
                IAgentHost armControlAgent = locator.GetAgent(this.armControlAgentName);
                this.ArmControlAgentPort = armControlAgent.GetControlPort<TArmCartesianControl>();
            }

            if (false == string.IsNullOrEmpty(this.handControlAgentName))
            {
                IAgentHost handControlAgent = locator.GetAgent(this.handControlAgentName);
                this.HandControlAgentPort = handControlAgent.GetControlPort<TGripperControl>();
            }
            
            if (false == string.IsNullOrEmpty(this.handStatusProvider))
            {
                IAgentHost hardwareAgentLocator = locator.GetAgent(this.handStatusProvider);
                hardwareAgentLocator.Subscribe<TGripperStatus>(this);
            }
            else
            {
                throw new ArgumentException("No hardware status agent provided");
            }
        }

        /// <summary>
        /// Receives an arm status message
        /// </summary>
        /// <param name="message">Arm status message</param>
        public override void Receive(ManipulatorStatusMessage message)
        {
            PicknPlaceStatusMessage msg = null;

            if (this.Context != null)
            {
                this.Context.CurrentArmPose = message.State.TcpCartesianPosition;
                string status = null;

                if (this.pickFromFSM != null)
                {
                    this.pickFromFSM.Step(this.Context);
                    status = this.pickFromFSM.ToString();
                }
                else if (this.placeToFSM != null)
                {
                    this.placeToFSM.Step(this.Context);
                    status = this.placeToFSM.ToString();
                }
                else if (null != this.Context.TargetTask)
                {
                    status = this.Context.TargetTask.Description;
                }

                if (null != this.Context.TargetTask && null != status)
                {
                    msg = new PicknPlaceStatusMessage(
                        message,
                        status,
                        this.Context.TargetTask.PicknPlaceStatus,
                        this.Context.ManipulatedObjectDescription,
                        this.Context.PickPlacePose,
                        this.Context.RetractPose,
                        this.Context.LastRequestMessage);
                }
            }
            
            if (msg == null)
            {
                msg = new PicknPlaceStatusMessage(
                    message, 
                    "Not started", 
                    PicknPlaceStatusEnum.NotStarted,
                    null,
                    new Pose(),
                    new Pose(),
                    null);
            }

            if (null != msg)
            {
                this.Publisher.Post(msg);
            }
        }

        /// <summary>
        /// Subscription receiver for hand messages
        /// </summary>
        /// <param name="message">The hand message</param>
        public abstract void Receive(TGripperStatus message);

        /// <summary>
        /// Handle requests for pick and and move. Even though the message can specify multiple pick and move requests,
        /// this handler acts only on the first request in the list.
        /// Generates all step configurations and execute
        /// Request is ignored unless a place task has just completed
        /// </summary>
        /// <param name="pickMessage">Message to process</param>
        public void ReceiveControl(PickAndMoveMessage pickMessage)
        {
            if (pickMessage.State.Length != 1)
            {
                TraceOut.Error(TraceContexts.Manip, "Pick and place agent expects 1 pickAndMove specification, found {0}", pickMessage.State.Length);
                return;
            }

            PickAndMove pickMove = pickMessage.State[0];
            Vector3 p = pickMove.PickFromPose.Position;
            Vector3 r = pickMove.MoveToPose.Position;
            Quaternion pq = pickMove.PickFromPose.Orientation;
            Quaternion rq = pickMove.MoveToPose.Orientation;
            TraceOut.Info(TraceContexts.Manip, "PickFrom: [{0:F3} {1:F3} {2:F3}]   [{3:f4} {4:F4} {5:F4} {6:F4}] {7}", p.X, p.Y, p.Z, pq.W, pq.X, pq.Y, pq.Z, pickMove.ManipulatedObjectDescription.Description);
            TraceOut.Info(TraceContexts.Manip, "RetractTo [{0:F3} {1:F3} {2:F3}]   [{3:f4} {4:F4} {5:F4} {6:F4}]", r.X, r.Y, r.Z, rq.W, rq.X, rq.Y, rq.Z);

            if (null == this.pickFromFSM && (null == this.Context || this.Context.TargetTask.PicknPlaceStatus == PicknPlaceStatusEnum.PlaceComplete || this.Context.TargetTask.PicknPlaceStatus == PicknPlaceStatusEnum.PickFailOutOfRange))
            {
                this.Context = new PicknPlaceFSMContext(
                    this.ArmOrientationMoveTolerance,
                    this.ArmPositionMoveTolerance,
                    pickMove.ManipulatedObjectDescription,
                    pickMove.PickFromPose,
                    pickMove.MoveToPose,
                    pickMessage);

                try
                {
                    Pose currentPose = this.defaultNeutralPose;

                    List<MoveAndGraspTaskDescription> pickTaskList = this.PicknPlaceGenerator.GetStepDescriptionsForPickTask(
                                            pickMove.ManipulatedObjectDescription,
                                            currentPose,
                                            pickMove.PickFromPose,
                                            pickMove.MoveToPose,
                                            this.liftHeight);

                    this.pickFromFSM = new MoveAndGraspFSM(this.PostArmMove, this.PostHandMove, pickTaskList.ToArray(), this.Context);
                }
                catch (NoGripAvailableException e)
                {
                    string message = string.Format("Cannot do pick from {0:F3} {1:F3} {2:F3}  RetractTo {3:F3} {4:F3} {5:F3} reason {6}", p.X, p.Y, p.Z, r.X, r.Y, r.Z, e.Message);
                    TraceOut.Info(TraceContexts.Manip, message);
                    this.pickFromFSM = null;
                    this.Context.TargetTask = new MoveAndGraspTaskDescription(new Pose(), PicknPlaceStatusEnum.PickFailOutOfRange, null, message, 0);
                }
                finally
                {
                    this.placeToFSM = null;
                }
            }
        }

        /// <summary>
        /// Handle requests for pick and and move.  Even though the message can specify multiple place and move requests,
        /// this handler acts only on the first request in the list
        /// Generate all step configurations and execute
        /// Request is ignored unless a pick task has recently completed
        /// </summary>
        /// <param name="placeMessage">Message to process</param>
        public void ReceiveControl(PlaceAndMoveMessage placeMessage)
        {
            if (placeMessage.State.Length != 1)
            {
                TraceOut.Error(TraceContexts.Manip, "Pick and place agent expects 1 pickAndMove specification, found {0}", placeMessage.State.Length);
                return;
            }

            PlaceAndMove placeMove = placeMessage.State[0];
            Vector3 p = placeMove.PlacePose.Position;
            Vector3 r = placeMove.RetractPose.Position;
            Quaternion pq = placeMove.PlacePose.Orientation;
            Quaternion rq = placeMove.RetractPose.Orientation;
            TraceOut.Info(TraceContexts.Manip, "PlaceTo: [{0:F3} {1:F3} {2:F3}]  [{3:f4} {4:F4} {5:F4} {6:F4}] {7}", p.X, p.Y, p.Z, pq.W, pq.X, pq.Y, pq.Z, placeMove.ManipulatedObjectDescription.Description);
            TraceOut.Info(TraceContexts.Manip, "RetractTo [{0:F3} {1:F3} {2:F3}] [{3:f4} {4:F4} {5:F4} {6:F4}]", r.X, r.Y, r.Z, rq.W, rq.X, rq.Y, rq.Z);

            GripStyle currentStyle = GripStyle.Unknown;

            if (null != this.Context && this.Context.TargetTask != null && this.Context.TargetTask.GripperState != null)
            {
                currentStyle = this.Context.TargetTask.GripperState.GripStyle;
            }

            if (null == this.placeToFSM && (null == this.Context || this.Context.TargetTask.PicknPlaceStatus == PicknPlaceStatusEnum.PickComplete || this.Context.TargetTask.PicknPlaceStatus == PicknPlaceStatusEnum.PlaceFailOutOfRange))
            {
                    this.Context = new PicknPlaceFSMContext(
                        this.ArmOrientationMoveTolerance, 
                        this.ArmPositionMoveTolerance, 
                        placeMove.ManipulatedObjectDescription,
                        placeMove.PlacePose,
                        placeMove.RetractPose,
                        placeMessage);
                try
                {
                    Pose currentPose = this.defaultNeutralPose;
                    Vector3 gp = this.GripExchangePose.Position;
                    Pose gripExchange = new Pose(new Vector3(gp.X, gp.Y, gp.Z + (placeMove.ManipulatedObjectDescription.HeightInM / 2.0)), this.GripExchangePose.Orientation);
                    List<MoveAndGraspTaskDescription> placeTaskList = this.PicknPlaceGenerator.GetStepDescriptionsForPlaceTask(
                                            placeMove.ManipulatedObjectDescription,
                                            currentStyle,
                                            currentPose,
                                            placeMove.PlacePose,
                                            placeMove.RetractPose,
                                            this.liftHeight,
                                            true,
                                            gripExchange);

                    this.placeToFSM = new MoveAndGraspFSM(this.PostArmMove, this.PostHandMove, placeTaskList.ToArray(), this.Context);
                }
                catch (NoGripAvailableException e)
                {
                    string message = string.Format("Cannot do place to {0:F3} {1:F3} {2:F3}  RetractTo {3:F3} {4:F3} {5:F3} Reason {6}", p.X, p.Y, p.Z, r.X, r.Y, r.Z, e.Message);
                    TraceOut.Info(TraceContexts.Manip, message);
                    this.placeToFSM = null;
                    this.Context.TargetTask = new MoveAndGraspTaskDescription(new Pose(), PicknPlaceStatusEnum.PlaceFailOutOfRange, null, message, 0);
                }
                finally
                {
                    this.pickFromFSM = null;
                }
            }
        }

        /// <summary>
        /// Publish a move an arm move message
        /// </summary>
        /// <param name="moveTo">Move to location</param>
        public abstract void PostArmMove(Pose moveTo);

        /// <summary>
        /// Publish a move a hand move message
        /// </summary>
        /// <param name="gripperState">Target gripper state</param>
        public abstract void PostHandMove(GripperStatus gripperState);
    }
}
